
class lqClientProxy:
	prev = None
	next = None
	bin = None
	object = None
	
	x = 0.0
	y = 0.0
	z = 0.0
	
class lqInternalDB:
    originx = 0.0
    originy = 0.0
    originz = 0.0

    sizex = 0.0
    sizey = 0.0
    sizez = 0.0

    divx = 0.0
    divy = 0.0
    divz = 0.0
    
    bins = []
    other = None
    
def lqCreateDatabase( _originx, _originy, _originz, _sizex, _sizey, _sizez, _divx, _divy, _divz ):
    _lq = lqInternalDB()
    lqInitDatabase( _lq, _originx, _originy, _originz, _sizex, _sizey, _sizez, _divx, _divy, _divz )
    
    return _lq
    
def lqInitDatabase( _lq, _originx, _originy, _originz, _sizex, _sizey, _sizez, _divx, _divy, _divz ):
    _lq.originx = _originx
    _lq.originy = _originy
    _lq.originz = _originz
    _lq.sizex = _sizex
    _lq.sizey = _sizey
    _lq.sizez = _sizez
    _lq.divx = _divx
    _lq.divy = _divy
    _lq.divz = _divz
    
    _binCount = _divx * _divy * _divz
    _lq.bins = [None for x in xrange(_binCount)]
    _lq.other = None
    
def lqBinCoordsToBinIndex( _lq, _ix, _iy, _iz ):
    return (_ix * _lq.divy * _lq.divz) + (_iy * _lq.divz + _iz)
    
def lqBinForLocation( _lq, _x, _y, _z ):
    if _x < _lq.originx: return _lq.other
    if _y < _lq.originy: return _lq.other
    if _z < _lq.originz: return _lq.other
    if _x >= (_lq.originx + _lq.sizex): return _lq.other
    if _y >= (_lq.originy + _lq.sizey): return _lq.other
    if _z >= (_lq.originz + _lq.sizez): return _lq.other
    
    _ix = int( ( (_x - _lq.originx) / _lq.sizex ) * _lq.divx )
    _iy = int( ( (_y - _lq.originy) / _lq.sizey ) * _lq.divy )
    _iz = int( ( (_z - _lq.originz) / _lq.sizez ) * _lq.divz )
    
    _Index = int( lqBinCoordsToBinIndex( _lq, _ix, _iy, _iz ) )
    
    return _lq.bins[_Index]
    
def lqInitClientProxy( _proxy, _clientObject ):
    _proxy.prev = None
    _proxy.next = None
    _proxy.bin = None
    _proxy.object = _clientObject
    
def lqAddToBin( _object, _bin ):
    if _bin == None:
        _object.prev = None
        _object.next = None
        _bin = _object
    else:
        _object.prev = None
        _object.next = _bin
        _bin.prev = _object
        _bin = _object
        
    _object.bin = _bin
    
def lqRemoveFromBin( _object ):
    if _object.bin != None:
        if _object.bin == _object: _object.bin = _object.next
        if _object.prev != None: _object.prev.next = _object.next
        if _object.next != None: _object.next.prev = _object.prev
        
    _object.prev = None
    _object.next = None
    _object.bin = None
    
def lqUpdateForNewLocation( _lq, _object, _x, _y, _z ):
    _newBin = lqBinForLocation( _lq, _x, _y, _z )
    
    _object.x = _x
    _object.y = _y
    _object.z = _z
    
    if _newBin != _object.bin:
        lqRemoveFromBin( _object )
        lqAddToBin( _object, _newBin )
        
def lqTraverseBinClientObjectList( _co, _x, _y, _z, _radiusSquared, _func, _state ):
    while _co != None:
        _dx = _x - _co.x
        _dy = _y - _co.y
        _dz = _z - _co.z
        _distanceSquared = (_dx * _dx) + (_dy * _dy) + (_dz * _dz)
        
        if _distanceSquared < _radiusSquared:
            _func( _co.object, _distanceSquared, _state )
            
        _co = _co.next
        
def lqMapOverAllObjectInLocalityClipped( _lq, _x, _y, _z, _radius, _func, _clientQueryState, \
                                        _minBinX, _minBinY, _minBinZ, _maxBinX, _maxBinY, _maxBinZ ):
    _iindex = 0
    _jindex = 0
    _kindex = 0
    _slab = int(_lq.divy * _lq.divz)
    _row = int(_lq.divz)
    _istart = _minBinX * _slab
    _jstart = _minBinY * _row
    _kstart = _minBinZ
    _radiusSquared = _radius * _radius
    _co = None
    _bin = None
    
    _iindex = int(_istart)
    for _i in xrange(_minBinX, _maxBinX + 1):
        _jindex = int(_jstart)
        for _j in xrange(_minBinY, _maxBinY + 1):
            _kindex = int(_kstart)
            for _k in xrange(_minBinZ, _maxBinZ + 1):
                _bin = _lq.bins[_iindex + _jindex + _kindex]
                _co = _bin
                
                lqTraverseBinClientObjectList( _co, _x, _y, _z, _radiusSquared, _func, _clientQueryState )
                
                _kindex += 1
            _jindex += _row
        _iindex += _slab
        
def lqMapOverAllOutsideObject( _lq, _x, _y, _z, _radius, _func, _clientQueryState ):
    _co = _lq.other
    _radiusSquared = _radius * _radius
    
    lqTraverseBinClientObjectList( _co, _x, _y, _z, _radiusSquared, _func, _clientQueryState )
    
def lqMapOverAllObjectsInLocality( _lq, _x, _y, _z, _radius, _func, _clientQueryState ):
    _partlyOut = False
    _completelyOutside = \
        ( (_x + _radius) < _lq.originx ) or \
        ( (_y + _radius) < _lq.originy ) or \
        ( (_z + _radius) < _lq.originz ) or \
        ( (_x - _radius) >= _lq.originx + _lq.sizex ) or \
        ( (_y - _radius) >= _lq.originy + _lq.sizey ) or \
        ( (_z - _radius) >= _lq.originz + _lq.sizez )
    
    if _completelyOutside:
        lqMapOverAllOutsideObject( _lq, _x, _y, _z, _radius, _func, _clientQueryState )
        return
        
    _minBinX = int( (((_x - _radius) - _lq.originx) / _lq.sizex) * _lq.divx )
    _minBinY = int( (((_y - _radius) - _lq.originy) / _lq.sizey) * _lq.divy )
    _minBinZ = int( (((_z - _radius) - _lq.originz) / _lq.sizez) * _lq.divz )
    _maxBinX = int( (((_x + _radius) - _lq.originx) / _lq.sizex) * _lq.divx )
    _maxBinY = int( (((_y + _radius) - _lq.originy) / _lq.sizey) * _lq.divy )
    _maxBinZ = int( (((_z + _radius) - _lq.originz) / _lq.sizez) * _lq.divz )
    
    if _minBinX < 0:
        _partlyOut = True
        _minBinX = 0
    if _minBinY < 0:
        _partlyOut = True
        _minBinY = 0
    if _minBinZ < 0:
        _partlyOut = True
        _minBinZ = 0
    if _maxBinX >= _lq.divx:
        _partlyOut = True
        _maxBinX = _lq.divx - 1
    if _maxBinY >= _lq.divy:
         _partlyOut = True
         _maxBinY = _lq.divy - 1
    if _maxBinZ >= _lq.divz:
        _partlyOut = True
        _maxBinZ = _lq.divz - 1
    
    if _partlyOut:
        lqMapOverAllOutsideObject( _lq, _x, _y, _z, _radius, _func, _clientQueryState )
    
    lqMapOverAllObjectInLocalityClipped( _lq, _x, _y, _z, _radius, _func, _clientQueryState, \
                                        _minBinX, _minBinY, _minBinZ, \
                                        _maxBinX, _maxBinY, _maxBinZ )
                                        
#-------------------------------------------------------------------------------------------------------------#
# internal helper function

class lqFindNearestState:
    ignoreObject = None
    nearestObject = None
    minDistanceSquared = 0.0
    
def lqFindNearestHelper( _clientObject, _distanceSuqared, _clientQueryState ):
    _fns = _clientQueryState
    
    if _fns != _clientObject:
        if _fns.minDistanceSquared > _distanceSuqared:
            _fns.nearestObject = _clientObject
            _fns.minDistanceSquared = _distanceSuqared
            
def lqFindNearestNeighborWithinRadius( _lq, _x, _y, _z, _radius, _ignoreObject ):
    _lqFNS = lqFindNearestState()
    _lqFNS.nearestObject = None
    _lqFNS.ignoreObject = _ignoreObject
    _minDistanceSquared = FLT_MAX
    
    lqMapOverAllObjectsInLocality( _lq, _x, _y, _z, _radius, lqFindNearestHelper, _lqFNS )
    
    return _lqFNS
    
def lqMapOverAllObjectsInBin( _binProxyList, _func, _clientQueryState ):
    while _binProxyList != None:
        _func( _binProxyList.object, 0, _clientQueryState )
        _binProxyList = _binProxyList.next
        
def lqMapOverAllObjectsInBin( _lq, _func, _clientQueryState ):
    _binCount = _lq.divx * _lq.divy * _lq.divz
    for i in xrange( _binCount ):
        lqMapOverAllObjectsInBin( _lq.bins[i], _func, _clientQueryState )
        
    lqMapOverAllObjectsInBin( _lq.other, _func, _clientQueryState )
    
def lqRemoveAllObjectInBin( _bin ):
    while _bin != None:
        lqRemoveFromBin( _bin )
        
def lqRemoveAllObjects( _lq ):
    _binCount = _lq.divx * _divy * _divz
    for i in xrange( _binCount ):
        lqRemoveAllObjectInBin( _lq.bins[i] )
    
    lqRemoveAllObjectInBin( _lq.other )